//
// Created by 张荣开 on 25-6-1.
//

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>

#define Type 1  //1:阿克曼，4:四轮四驱    //ET_01小车只有阿克曼前转向模式

using namespace std;
using std::placeholders::_1;

class CmdVelTransNode : public rclcpp::Node
{
public:
    CmdVelTransNode() : Node("cmd_vel_trans")
    {
        // 订阅者类型改为Twist，与回调函数参数类型一致
        cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
            "/twist_cmd", 10, std::bind(&CmdVelTransNode::cmd_velCallback, this, _1));
            
        cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
        
        RCLCPP_INFO(this->get_logger(), "CmdVelTransNode initialized");
    }

private:
    // 订阅者和发布者
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;  // 修正：改为Twist类型
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
    
    // 速度变量
    double vx = 0.0;
    double vth = 0.0;
    
    // 阿克曼前后轮距离
    const float WHELLBASE = 0.785;
    
    // 填充速度消息
    geometry_msgs::msg::Twist velocity;
    
    // 计算阿克曼转角弧度
    float angz_to_angle(float Vx, float Vz)
    {
        float R = 0;
        if (Vx == 0 || Vz == 0)
        {
            return 0;
        }
        
        R = Vx / Vz;
        return atan(WHELLBASE / R);
    }
    
    // 回调函数
    void cmd_velCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
    {
        vx = msg->linear.x;
        vth = msg->angular.z;

        velocity.linear.x = vx;
#if   Type == 1
        velocity.angular.z = angz_to_angle(vx, vth);//用阿克曼公式 
#elif Type == 4
        velocity.angular.z = vth;
#endif

        RCLCPP_INFO(this->get_logger(), "vx: %f, vth: %f", velocity.linear.x, velocity.angular.z);

        cmd_vel_pub_->publish(velocity);
    }
};

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<CmdVelTransNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
